/**
 * @file countdown_action_server.cpp
 * @brief ROS2 Action Server 示例：实现倒计时功能
 * 
 * 本文件演示了如何创建一个 ROS2 Action Server：
 * 1. 接收倒计时目标（Goal）
 * 2. 执行倒计时并发布反馈（Feedback）
 * 3. 返回最终结果（Result）
 * 4. 支持取消操作
 */

#include <rclcpp/rclcpp.hpp>
#include <rclcpp_action/rclcpp_action.hpp>
#include <thread>
#include <chrono>
#include <memory>

#include "simple_action_demo/action/countdown.hpp"

using namespace std::chrono_literals;

/**
 * @class CountdownActionServer
 * @brief 倒计时 Action Server 类
 * 
 * 继承自 rclcpp::Node，实现倒计时 Action 服务端功能
 */
class CountdownActionServer: public rclcpp::Node
{
public:
    // 类型别名，简化代码
    using Countdown = simple_action_demo::action::Countdown;
    using GoalHandleCountdown = rclcpp_action::ServerGoalHandle<Countdown>;

    /**
     * @brief 构造函数
     * @param options ROS2 节点选项
     * 
     * 初始化 Action Server，注册三个关键回调函数：
     * - handle_goal: 处理目标请求，决定是否接受
     * - handle_cancel: 处理取消请求
     * - handle_accepted: 目标被接受后执行任务
     */
    explicit CountdownActionServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions())
    :Node("countdown_action_server", options)
    {
        // 创建 Action Server
        // 参数说明：
        // - this: 当前节点指针
        // - "countdown": Action 名称（客户端通过此名称连接）
        // - handle_goal: 目标处理回调（接受/拒绝目标）
        // - handle_cancel: 取消处理回调
        // - handle_accepted: 目标接受后执行回调
        action_server_ = rclcpp_action::create_server<Countdown>(
            this,
            "countdown",
            std::bind(&CountdownActionServer::handle_goal, this, std::placeholders::_1, std::placeholders::_2),
            std::bind(&CountdownActionServer::handle_cancel, this, std::placeholders::_1),
            std::bind(&CountdownActionServer::handle_accepted, this, std::placeholders::_1)
        );
    }
private:
    rclcpp_action::Server<Countdown>::SharedPtr action_server_;  // Action Server 实例

    /**
     * @brief 处理目标请求回调函数
     * @param uuid 目标的唯一标识符
     * @param goal 目标消息（包含 start_number）
     * @return GoalResponse 接受或拒绝目标
     * 
     * 当客户端发送目标时，此函数会被调用。
     * 可以在这里验证目标的合法性，决定是否接受。
     * 
     * 返回值：
     * - ACCEPT_AND_EXECUTE: 接受目标并立即执行
     * - ACCEPT_AND_DEFER: 接受目标但延迟执行
     * - REJECT: 拒绝目标
     */
    rclcpp_action::GoalResponse handle_goal(
        const rclcpp_action::GoalUUID& uuid,
        std::shared_ptr<const Countdown::Goal> goal)
    {
        RCLCPP_INFO(this->get_logger(), "Received goal request with start number: %d", goal->start_number);
        
        // 验证目标：起始数字必须为正数
        if(goal->start_number <=0)
        {
            RCLCPP_WARN(this->get_logger(),"Goal rejected: start number must be positive");
            return rclcpp_action::GoalResponse::REJECT;
        }
        
        // 验证目标：起始数字不能超过最大值（防止执行时间过长）
        if(goal->start_number > 20)
        {
            RCLCPP_WARN(this->get_logger(), "Goal rejected: start number too large (max 20)");
            return rclcpp_action::GoalResponse::REJECT;
        }
        
        (void)uuid; // 避免未使用参数警告
        RCLCPP_INFO(this->get_logger(), "Goal accepted");
        return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
    }

    /**
     * @brief 处理取消请求回调函数
     * @param goal_handle 目标句柄
     * @return CancelResponse 是否接受取消请求
     * 
     * 当客户端请求取消目标时，此函数会被调用。
     * 可以在这里实现取消逻辑（如清理资源）。
     */
    rclcpp_action::CancelResponse handle_cancel(
        const std::shared_ptr<GoalHandleCountdown> goal_handle)
    {
        RCLCPP_INFO(this->get_logger(),"Received request to cancel goal");
        (void) goal_handle;
        // 返回 ACCEPT 表示接受取消请求
        // 返回 REJECT 表示拒绝取消（任务可能已经完成或无法取消）
        return rclcpp_action::CancelResponse::ACCEPT;
    }

    /**
     * @brief 目标被接受后的回调函数
     * @param goal_handle 目标句柄
     * 
     * 当目标被接受后，此函数会被调用。
     * 在这里启动实际的任务执行（倒计时）。
     * 
     * 注意：使用新线程执行任务，避免阻塞主线程。
     * 这样可以同时处理多个目标请求。
     */
    void handle_accepted(const std::shared_ptr<GoalHandleCountdown> goal_handle)
    {
        // 在新线程中执行倒计时，避免阻塞主线程
        // detach() 使线程在后台运行，不等待其完成
        std::thread{std::bind(&CountdownActionServer::execute_countdown, this, std::placeholders::_1), goal_handle}.detach();
    }
    /**
     * @brief 执行倒计时任务
     * @param goal_handle 目标句柄
     * 
     * 这是实际执行倒计时的函数：
     * 1. 从目标中获取起始数字
     * 2. 循环倒计时，每秒发布一次反馈
     * 3. 检查是否被取消
     * 4. 完成后发送结果
     */
    void execute_countdown(const std::shared_ptr<GoalHandleCountdown> goal_handle)
    {
        RCLCPP_INFO(this->get_logger(),"Executing countdown...");
        
        // 从目标句柄中获取目标消息
        const auto goal = goal_handle->get_goal();
        
        // 创建反馈和结果消息
        auto feedback = std::make_shared<Countdown::Feedback>();
        auto result = std::make_shared<Countdown::Result>();
        
        // 初始化当前数字为起始数字
        int32_t current = goal->start_number;

        // 倒计时循环：从起始数字到 0
        while(rclcpp::ok() && current >=0)
        {
            // 检查是否收到取消请求
            if(goal_handle->is_canceling()){
                // 清理结果序列
                result->countdown_sequence.clear();
                // 通知客户端任务已取消
                goal_handle->canceled(result);
                RCLCPP_INFO(this->get_logger(), "Countdown canceled");
                return;
            }
            
            // 更新反馈消息
            feedback->current_number = current;
            // 发布反馈给客户端（客户端可以在 feedback_callback 中接收）
            goal_handle->publish_feedback(feedback);
            RCLCPP_INFO(this->get_logger(), "Countdown: %d", current);
            
            // 保存当前数字到结果序列
            result->countdown_sequence.push_back(current);
            
            // 等待1秒（模拟倒计时间隔）
            std::this_thread::sleep_for(1s);
            current--;
        }
        
        // 检查是否成功完成（ROS2 仍在运行且未被取消）
        if (rclcpp::ok()) {
            result->countdown_sequence.push_back(0); // 添加最后的0
            // 通知客户端任务成功完成，并发送结果
            goal_handle->succeed(result);
            RCLCPP_INFO(this->get_logger(), "Countdown completed successfully!");
        }
    }
};

/**
 * @brief 主函数
 * @param argc 命令行参数数量
 * @param argv 命令行参数数组
 * @return 程序退出码
 * 
 * 创建并运行 Action Server 节点
 */
int main(int argc,char**argv)
{
    // 初始化 ROS2
    rclcpp::init(argc,argv);
    
    // 创建 Action Server 节点
    auto node = std::make_shared<CountdownActionServer>();
    
    // 进入事件循环，处理回调函数
    rclcpp::spin(node);
    
    // 关闭 ROS2
    rclcpp::shutdown();
    return 0;
}
